#!/usr/bin/env roseus

(jsk)
(rbrain)

(ros::load-ros-manifest "jskpointcloud")
(load "package://jskpointcloud/euslisp/jskpointcloud.l")
(ros::load-ros-manifest "human_tracking")
(ros::roseus "hoge")

(defvar *r-max* 255)
(defvar *r-min* 130)
(defvar *g-max* 100)
(defvar *g-min* 0)
(defvar *b-max* 100)
(defvar *b-min* 0)
(defvar *num-thr* 0)

(setq *pub-people-topic* "people_pcloud")
(ros::advertise *pub-people-topic* sensor_msgs::PointCloud2 1)

(setq *pub-people-topic-filtered* "people_pcloud_filtered")
(ros::advertise *pub-people-topic-filtered* sensor_msgs::PointCloud2 1)

(defun pcloud-cb (msg)
  (let ((3dp
         (make-3dpointcloud-from-msg2 msg :remove-nan nil))
        (3dp-lst (list)))
    (when (car *indices*)
      (setq hoge 3dp)
      (let ((p (send 3dp :filter-with-indices (coerce (car *indices*) cons) :create t))
            pub-pc1 pub-pc2)
        (setq pub-pc1 (make-msg2-from-3dpointcloud
                        p :frame (send msg :header :frame_id)))
        (ros::publish *pub-people-topic* pub-pc1)
        (send p :nfilter :key
              #'(lambda (x)
                  (not (and (< 0 (elt x 0)) (< (elt x 0) 0)))))
        (setq p (is-person-with-color-object p))
        ;;(setq p (is-person-with-color-object p))
        (setq pub-pc2 (make-msg2-from-3dpointcloud
                       p :frame (send msg :header :frame_id)))
        (ros::publish *pub-people-topic-filtered* pub-pc2)))))

(defun is-person-with-color-object (human-3dp)
  (send human-3dp :nfilter :ckey
        #'(lambda (x)
            (let ((col (scale 255 x)))
              (and (< *r-min* (elt col 0) *r-max*)
                   (< *g-min* (elt col 1) *g-max*)
                   (< *b-min* (elt col 2) *b-max*))))
        :create t))

(defun index-cb (msg)
  (let ((hdr (send msg :header))
        (lst (send msg :array)))
    (pprint (list 'index (length lst)))
    (setq *indices* nil)
    (dolist (l lst)
      (push (send l :indices) *indices*))))

(ros::subscribe "/camera/depth_registered/points" sensor_msgs::PointCloud2 #'pcloud-cb)
(ros::subscribe "/openni_tracker/indices" human_tracking::IndicesArray #'index-cb)

(setq *indices* nil)

(defun ros-spin ()
  (do-until-key (ros::spin-once)))

(ros-spin)